import sensor, image, time
from pyb import UART
from pyb import LED

THRESHOLD = (0, 22, -128, 127, -128, 127)

SCREEN_W = 80
SCREEN_H = 60
ROI_W = 20
ROI_H = 50
ROI = (int(SCREEN_W/2 - ROI_W/2), int(SCREEN_H/2-ROI_H/2), ROI_W, ROI_H)

uart = UART(3, 115200)

LED(1).on()
LED(2).on()
LED(3).on()

sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)

sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
#sensor.set_windowing([0,20,80,40])
sensor.skip_frames()     # WARNING: If you use QQVGA it may take seconds
clock = time.clock()                # to process a frame sometimes.

while(True):
    clock.tick()

    img = sensor.snapshot()
    img = img.binary([THRESHOLD])
    roi_img = img.copy(roi=ROI, copy_to_fb=False)
    img.draw_rectangle(ROI, color = 150)

    line = roi_img.get_regression([(100,100)], robust = True)
    if (line):

        # rho_err = abs(line.rho())-roi_img.width()/2

        if line.theta()>90:
            theta_err = line.theta()-180
        else:
            theta_err = line.theta()

        # 画线性回归的线
        line_list = list(line.line())
        offset_x = int(SCREEN_W / 2 - ROI_W/2)
        offset_y = int(SCREEN_H / 2 - ROI_H/2)
        line_list[0] += offset_x;
        line_list[1] += offset_y;
        line_list[2] += offset_x;
        line_list[3] += offset_y;
        img.draw_line(tuple(line_list), color = 127)

        # print(rho_err,line.magnitude())

        if line.magnitude()>8:  # 线性回归效果好
            if theta_err > 20:  # 右转
                print("b")
                uart.write("b")
            elif theta_err > 10:
                print("a")
                uart.write("a")
            elif theta_err < -20:  # 左转
                print("d")
                uart.write("d");
            elif theta_err < -10:
                print("c")
                uart.write("c")
            else:
                print("f")
                uart.write("f")
        else:
            print("n")
            uart.write("n")
    else:
        print("x")
        uart.write("x")

    #print(clock.fps())
